package collision;

import geometry.Triangle;
import geometry.TriangleMesh;

import java.util.HashMap;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

public class ASDFOctreeNode
{
    public static final double ASDF_TOLERANCE = 0.02;

    private ASDFOctree _tree;
    // 0: (0,0,0)
    // 1: (0,0,1)
    // 2: (0,1,0)
    // 3: (0,1,1)
    // 4: (1,0,0)
    // 5: (1,0,1)
    // 6: (1,1,0)
    // 7: (1,1,1)
    private ASDFOctreeNode[] _children;
    private double[] _signedDist = new double[8];
    // private Point3d[] _targetPoints = new Point3d[8];
    private Point3d _minPt = new Point3d();
    private Point3d _maxPt = new Point3d();
    private Point3d _centerPt = new Point3d();
    private Point3d _maxSubMinInv = new Point3d();
    private static int nodeCount = 0;
    private static int curLevel = 0;
    private static int maxLevel = 0;
    private static int insideCount = 0;
    private static int outsideCount = 0;
    private static int closeCount = 0;
    
    // AABB is used for quick construction of an axis-aligned OBB.
 	public static OBB AABB;
 	
 	static
 	{
 		AABB = new OBB();
 		AABB.axes[0] = new Vector3d(1, 0, 0);
 		AABB.axes[1] = new Vector3d(0, 1, 0);
 		AABB.axes[2] = new Vector3d(0, 0, 1);
 	}
    
    public ASDFOctreeNode(ASDFOctree tree_, ASDFOctreeNode parent_,
            Point3d minPt_, Point3d maxPt_)
    {
//         for (int i = 0; i < 8; i++)
//         _targetPoints[i] = new Point3d();
//    	System.out.println("minpt " + minPt_ + " maxpt " + maxPt_);
        _minPt.set(minPt_);
        _maxPt.set(maxPt_);
        _maxSubMinInv.x = 1.0 / (_maxPt.x - _minPt.x);
        _maxSubMinInv.y = 1.0 / (_maxPt.y - _minPt.y);
        _maxSubMinInv.z = 1.0 / (_maxPt.z - _minPt.z);
        Vector3d halfDiag = new Vector3d();
        halfDiag.sub(_maxPt, _minPt);
        halfDiag.scale(0.5);
        _centerPt.add(_minPt, halfDiag);
        _tree = tree_;
        _signedDist[0] = computeSignedDistance(_minPt);
        _signedDist[1] = computeSignedDistance(new Point3d(_minPt.getX(),
                _minPt.getY(), _maxPt.getZ()));
        _signedDist[2] = computeSignedDistance(new Point3d(_minPt.getX(),
                _maxPt.getY(), _minPt.getZ()));
        _signedDist[3] = computeSignedDistance(new Point3d(_minPt.getX(),
                _maxPt.getY(), _maxPt.getZ()));
        _signedDist[4] = computeSignedDistance(new Point3d(_maxPt.getX(),
                _minPt.getY(), _minPt.getZ()));
        _signedDist[5] = computeSignedDistance(new Point3d(_maxPt.getX(),
                _minPt.getY(), _maxPt.getZ()));
        _signedDist[6] = computeSignedDistance(new Point3d(_maxPt.getX(),
                _maxPt.getY(), _minPt.getZ()));
        _signedDist[7] = computeSignedDistance(new Point3d(_maxPt.getX(),
                _maxPt.getY(), _maxPt.getZ()));

        if (curLevel > maxLevel)
            maxLevel = curLevel;
        curLevel++;
        splitASDFOctreeNode();
        curLevel--;
    }

    private double computeSignedDistance(Point3d pt_)
    {
    	double d = _tree.findCachedDistance(pt_);
    	if (d != Double.MAX_VALUE)
    		return d;
    	else
    	{
	        Triangle intersectingTriangle = new Triangle();
	        Point3d targetPoint = new Point3d();
	        
	        if (pt_.x == -0.25 && pt_.y == -0.5 && pt_.z == -0.5)
	        {
	        	int k = 0;
	        }

	        double signedDistance = _tree.obbtree.getSignedDistance(pt_);
	        
//	        System.out.println("signed distance for " + pt_ + " is " + signedDistance);
//	        double signedDistance2 = _tree.obbtree.geometry.getTriangleMesh().distanceToPoint(pt_,
//	                intersectingTriangle, targetPoint);
//	        
//	        if (Math.abs(signedDistance2 - Math.abs(signedDistance)) > 1e-10)
//	        {
//	        	int k = 0;
//	        }

	        _tree.addCachedDistance(pt_, signedDistance);
	        
	        return signedDistance;
    	}
    }

    public double getDistance(Point3d pt, Vector3d normal)
    {
        // If we have children, we recursively obtain the distance.
        if (_children != null)
        {
            if (pt.getX() < _centerPt.getX())
            {
                if (pt.getY() < _centerPt.getY())
                {
                    if (pt.getZ() < _centerPt.getZ())
                    {
                        return _children[0].getDistance(pt, normal);
                    }
                    else
                    {
                        return _children[1].getDistance(pt, normal);
                    }
                }
                else
                {
                    if (pt.getZ() < _centerPt.getZ())
                    {
                        return _children[2].getDistance(pt, normal);
                    }
                    else
                    {
                        return _children[3].getDistance(pt, normal);
                    }
                }
            }
            else
            {
                if (pt.getY() < _centerPt.getY())
                {
                    if (pt.getZ() < _centerPt.getZ())
                    {
                        return _children[4].getDistance(pt, normal);
                    }
                    else
                    {
                        return _children[5].getDistance(pt, normal);
                    }
                }
                else
                {
                    if (pt.getZ() < _centerPt.getZ())
                    {
                        return _children[6].getDistance(pt, normal);
                    }
                    else
                    {
                        return _children[7].getDistance(pt, normal);
                    }
                }
            }
        }
        // Otherwise, we trilinearly interpolate.
        else
        {
        	// Normal = the gradient of distance.
        	Point3d tmpMin = new Point3d(pt);
        	Point3d tmpMax = new Point3d(pt);
        	
        	tmpMin.x = _minPt.x;
        	tmpMax.x = _maxPt.x;
        	normal.x = interpolateDistance(tmpMax) - interpolateDistance(tmpMin);
        	tmpMin.x = pt.x;
        	tmpMax.x = pt.x;
        	
        	tmpMin.y = _minPt.y;
        	tmpMax.y = _maxPt.y;
        	normal.y = interpolateDistance(tmpMax) - interpolateDistance(tmpMin);
        	tmpMin.y = pt.y;
        	tmpMax.y = pt.y;
        	
        	tmpMin.z = _minPt.z;
        	tmpMax.z = _maxPt.z;
        	normal.z = interpolateDistance(tmpMax) - interpolateDistance(tmpMin);
        	
        	return interpolateDistance(pt);
        }
    }
    
    private double interpolateDistance(Point3d pt)
    {
    	double aX = (pt.x - _minPt.x) * _maxSubMinInv.x;
        double aY = (pt.y - _minPt.y) * _maxSubMinInv.y;
        double aZ = (pt.z - _minPt.z) * _maxSubMinInv.z;

        double d0 = (1 - aX) * (1 - aY) * (1 - aZ) * _signedDist[0];
        double d1 = (1 - aX) * (1 - aY) * aZ * _signedDist[1];
        double d2 = (1 - aX) * aY * (1 - aZ) * _signedDist[2];
        double d3 = (1 - aX) * aY * aZ * _signedDist[3];
        double d4 = aX * (1 - aY) * (1 - aZ) * _signedDist[4];
        double d5 = aX * (1 - aY) * aZ * _signedDist[5];
        double d6 = aX * aY * (1 - aZ) * _signedDist[6];
        double d7 = aX * aY * aZ * _signedDist[7];

        return d0 + d1 + d2 + d3 + d4 + d5 + d6 + d7;
    }

    private boolean containsPartOfMesh(OBBTree obbtree)
    {
        AABB.center.set(_centerPt);
        AABB.size[0] = _maxPt.x - _centerPt.x;
        AABB.size[1] = _maxPt.y - _centerPt.y;
        AABB.size[2] = _maxPt.z - _centerPt.z;
        return obbtree.isIntersectedWithOBB(AABB);
    }

    public boolean containsPoint(Point3d p)
    {
        return _minPt.x <= p.x && p.x <= _maxPt.x && _minPt.y <= p.y
                && p.y <= _maxPt.y && _minPt.z <= p.z && p.z <= _maxPt.z;
    }

    private void splitASDFOctreeNode()
    {
        boolean insideRigidBody = (_signedDist[0] < 0 && _signedDist[1] < 0
                && _signedDist[2] < 0 && _signedDist[3] < 0
                && _signedDist[4] < 0 && _signedDist[5] < 0
                && _signedDist[6] < 0 && _signedDist[7] < 0);
        boolean outsideRigidBody = !containsPartOfMesh(_tree.obbtree)
                && _signedDist[0] > 0 && _signedDist[1] > 0
                && _signedDist[2] > 0 && _signedDist[3] > 0
                && _signedDist[4] > 0 && _signedDist[5] > 0
                && _signedDist[6] > 0 && _signedDist[7] > 0;
        boolean signedDistTooSmall = (Math.abs(_signedDist[0]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[1]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[2]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[3]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[4]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[5]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[6]) < ASDF_TOLERANCE)
                && (Math.abs(_signedDist[7]) < ASDF_TOLERANCE);

        // Check if we can approximate the center point distance with trilinear interpolation.
        boolean trilinearInterpolatable = false;
        Point3d centerPoint = new Point3d();
        Vector3d halfDiag = new Vector3d();
        halfDiag.sub(_maxPt, _minPt);
        halfDiag.scale(0.5);
        centerPoint.add(_minPt, halfDiag);
        if (Math.abs(interpolateDistance(centerPoint) - computeSignedDistance(centerPoint)) < ASDF_TOLERANCE)
        	trilinearInterpolatable = true;
        
        boolean smallCell = (_maxPt.x - _minPt.x < ASDF_TOLERANCE
        				&&   _maxPt.y - _minPt.y < ASDF_TOLERANCE
        				&&   _maxPt.z - _minPt.z < ASDF_TOLERANCE);

        nodeCount++;
        if (insideRigidBody || outsideRigidBody || trilinearInterpolatable || smallCell)
        {
            // Stop subdividing when entire rectangle is outside the object.
            if (signedDistTooSmall)
            {
                closeCount++;
                // System.out.println(_minPt.toString() + _maxPt.toString());
            }
            else if (insideRigidBody)
                insideCount++;
            else if (outsideRigidBody)
                outsideCount++;
            return;
        }
        else
        {
            Point3d currentMinPoint = new Point3d();
            Point3d currentMaxPoint = new Point3d();
            Vector3d eX = new Vector3d(halfDiag.getX(), 0.0, 0.0);
            Vector3d eY = new Vector3d(0.0, halfDiag.getY(), 0.0);
            Vector3d eZ = new Vector3d(0.0, 0.0, halfDiag.getZ());
            Vector3d eXY = new Vector3d(halfDiag.getX(), halfDiag.getY(), 0.0);
            Vector3d eYZ = new Vector3d(0.0, halfDiag.getY(), halfDiag.getZ());
            Vector3d eXZ = new Vector3d(halfDiag.getX(), 0.0, halfDiag.getZ());
            Vector3d eXYZ = new Vector3d(halfDiag.getX(), halfDiag.getY(),
                    halfDiag.getZ());

            _children = new ASDFOctreeNode[8];
            _children[0] = new ASDFOctreeNode(_tree, this, _minPt, centerPoint);
            currentMinPoint.add(_minPt, eZ);
            currentMaxPoint.add(centerPoint, eZ);
            _children[1] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eY);
            currentMaxPoint.add(centerPoint, eY);
            _children[2] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eYZ);
            currentMaxPoint.add(centerPoint, eYZ);
            _children[3] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eX);
            currentMaxPoint.add(centerPoint, eX);
            _children[4] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eXZ);
            currentMaxPoint.add(centerPoint, eXZ);
            _children[5] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eXY);
            currentMaxPoint.add(centerPoint, eXY);
            _children[6] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
            currentMinPoint.add(_minPt, eXYZ);
            currentMaxPoint.add(centerPoint, eXYZ);
            _children[7] = new ASDFOctreeNode(_tree, this, currentMinPoint,
                    currentMaxPoint);
        }
    }

    public ASDFOctreeNode[] getChildren()
    {
        return _children;
    }

    public Point3d getMaxPt()
    {
        return _maxPt;
    }

    public void setMaxPt(Point3d maxPt_)
    {
        _maxPt = maxPt_;
    }

    public Point3d getMinPt()
    {
        return _minPt;
    }

    public void setMinPt(Point3d minPt_)
    {
        _minPt = minPt_;
    }
}
